#!/usr/bin/env python
import roslib; roslib.load_manifest('circle_detector')
import rospy

from std_msgs.msg import String
from sensor_msgs.msg import Image
from background_filters.msg import ObjectList

from cv_bridge import CvBridge, CvBridgeError

import sys

import cv

class ShapeDetector:

    def __init__(self):
        cv.NamedWindow("win", 0)
        cv.ResizeWindow("win", 128, 128)
        self.font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("object_list", ObjectList, self.callback)

    def callback(self, data):
        #print len(data.color_images)
        #for roi in data.object_regions:
        #    print roi    
        for i in range(0, len(data.color_images)):
            try:
                color_image = data.color_images[i]
                bw_image = data.bw_images[i]
            
                cv_image = self.bridge.imgmsg_to_cv(color_image, "bgr8")
                small_image = cv.CreateImage((128,128), 8, 3)
                cv.Resize(cv_image, small_image)
                cv.PutText(small_image, data.shapes[i] + " " + str(data.ratios[i]), (2, 100), self.font, cv.Scalar(0, 0, 255, 0))
                
                cv_image2 = self.bridge.imgmsg_to_cv(bw_image, "mono8")
                small_image2 = cv.CreateImage((128,128), 8, 1)
                cv.Resize(cv_image2, small_image2)
                
                #sumPixels = cv.Sum(small_image2)[0]
                
                #print sumPixels
                #if sumPixels > 500000:
                
                #print 
                
                cv.ShowImage("win", small_image)
                cv.WaitKey(1000)
                
                cv.ShowImage("win", small_image2)
                cv.WaitKey(1000)
                
            except CvBridgeError, e:
                print e 
    
        rospy.signal_shutdown("Done!")
        

#    (cols,rows) = cv.GetSize(cv_image)
#    if cols > 60 and rows > 60 :
#      cv.Circle(cv_image, (50,50), 10, 255)

    
#    cv.WaitKey(3)

#    try:
#      self.image_pub.publish(self.bridge.cv_to_imgmsg(cv_image, "bgr8"))
#    except CvBridgeError, e:
#      print e

if __name__ == '__main__':
    ic = ShapeDetector()
    rospy.init_node('shape_detector', anonymous=True)
    
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down"
    
    cv.DestroyAllWindows()
